#include "Motor.h"

Motor::Motor()
{
}

Motor::~Motor()
{
}

 /////////////////////////////////////////////////
/**
 * 控制机器人向前移动
 * @param speed 电机速度，范围为0-255
  * 
 * 接线方式：
 * 同侧车轮电机两个引脚正负一致
 * 对侧车轮两个引脚正负相反
 * 这种安装方式可以保证轮子控制方向统一
  * 
 * ‌前进/后退‌：所有轮子同向旋转（如正转或反转）；
 * ‌侧移‌：同一侧两个轮子反向旋转，另一侧两个轮子同向旋转；
 * ‌原地旋转‌：对角线轮子反向旋转，实现顺时针或逆时针旋转；
 * ‌斜向滑行‌：调整单个轮子转速和方向，组合实现斜向移动。
 * 
 * 速度控制(以左前轮为例):
 * 当IN_2为LOW时为正转，IN_1的值越大，速度越快
 * 当IN_2为HIGH时为反转，IN_1的值越小，速度越快
 * 所以当设置速度时,如果IN_2为HIGH,则需要用255-speed来设置IN_1的值
 */

 /////////////////////////////////////////////////
//‌前进/后退‌：所有轮子同向旋转
void Motor::goAhead(int speed)
{
  analogWrite(IN_1, 255-speed);
  digitalWrite(IN_2, HIGH);

  analogWrite(IN_5, 255-speed);
  digitalWrite(IN_6, HIGH);

  analogWrite(IN_3, 255-speed);
  digitalWrite(IN_4, HIGH);

  analogWrite(IN_7, 255-speed);
  digitalWrite(IN_8, HIGH);
}

void Motor::goBack(int speed)
{
  // 左前轮
  analogWrite(IN_1, speed);
  digitalWrite(IN_2, LOW);   //LOW为正转,快速衰减  HIGH为反转,慢速衰减
  // 左后轮
  analogWrite(IN_5, speed);
  digitalWrite(IN_6, LOW);
  // 右前轮
  analogWrite(IN_3, speed);
  digitalWrite(IN_4, LOW);
  // 右后轮
  analogWrite(IN_7, speed);
  digitalWrite(IN_8, LOW);
}

 /////////////////////////////////////////////////
//‌侧移‌：同一侧两个轮子反向旋转，另一侧两个轮子同向旋转
void Motor::goRight(int speed)
{
  analogWrite(IN_1, 255-speed);
  digitalWrite(IN_2, HIGH);
  
  analogWrite(IN_5, speed);
  digitalWrite(IN_6, LOW);

  analogWrite(IN_3, speed);
  digitalWrite(IN_4, LOW);

  analogWrite(IN_7, 255-speed);
  digitalWrite(IN_8, HIGH);
  
  printf("向左平移");
}

void Motor::goLeft(int speed)
{
  analogWrite(IN_1, speed);
  digitalWrite(IN_2, LOW);

  analogWrite(IN_5, 255-speed);
  digitalWrite(IN_6, HIGH);

  analogWrite(IN_3, 255-speed);
  digitalWrite(IN_4, HIGH);
  
  analogWrite(IN_7, speed);
  digitalWrite(IN_8, LOW);

  printf("向右平移");
}

/////////////////////////////////////////////////
//‌斜向滑行‌：调整单个轮子转速和方向
void Motor::goAheadRight(int speed)
{
  stopRobot();

  analogWrite(IN_1, 255-speed);
  digitalWrite(IN_2, HIGH);
  
  analogWrite(IN_7, 255-speed);
  digitalWrite(IN_8, HIGH); 
}

void Motor::goAheadLeft(int speed)
{
  stopRobot();

  analogWrite(IN_5, 255-speed);
  digitalWrite(IN_6, HIGH);

  analogWrite(IN_3, 255-speed);
  digitalWrite(IN_4, HIGH);
}

void Motor::goBackLeft(int speed)
{
  stopRobot();

  analogWrite(IN_1, speed);
  digitalWrite(IN_2, LOW);

  analogWrite(IN_7, speed);
  digitalWrite(IN_8, LOW);
}

void Motor::goBackRight(int speed)
{
  stopRobot();

  analogWrite(IN_5, speed);
  digitalWrite(IN_6, LOW);

  analogWrite(IN_3, speed);
  digitalWrite(IN_4, LOW);
}

void Motor::stopRobot()
{
  digitalWrite(IN_1, LOW);
  digitalWrite(IN_2, LOW);
  digitalWrite(IN_3, LOW);
  digitalWrite(IN_4, LOW);
  digitalWrite(IN_5, LOW);
  digitalWrite(IN_6, LOW);
  digitalWrite(IN_7, LOW);
  digitalWrite(IN_8, LOW);
}

/////////////////////////////////////////////////
//原地旋转: 对角轮子反向旋转，实现顺时针或逆时针旋转
void Motor::turnLeft(int speed)
{
  analogWrite(IN_1, speed);
  digitalWrite(IN_2, LOW);

  analogWrite(IN_5, speed);
  digitalWrite(IN_6, LOW);

  analogWrite(IN_3, 255-speed);
  digitalWrite(IN_4, HIGH);

  analogWrite(IN_7, 255-speed);
  digitalWrite(IN_8, HIGH);

  printf("向左转");
}

void Motor::turnRight(int speed)
{
  analogWrite(IN_1, 255-speed);
  digitalWrite(IN_2, HIGH);

  analogWrite(IN_5, speed);
  digitalWrite(IN_6, HIGH);

  analogWrite(IN_3, 255-speed);
  digitalWrite(IN_4, LOW);

  analogWrite(IN_7, speed);
  digitalWrite(IN_8, LOW);

  printf("向右转");
}

Motor motor;